#!/usr/bin/python
# coding=utf-8
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from lejulib import *
import numpy as np
from motion.motionControl import *

GAIT_RANGE = 0.05 #步态的范围
ROTATION_RANGE = 10.0 
ROI = (100, 100) #tuple类型的数组 相当于const的作用无法修改


def slow_walk(direction, stepnum=1, angle=None):
    """
    :param direction: "forward" ,"backward" or "rotation"
    :param stepnum: int num
    :return:
    """
    array = [0.0, 0.0, 0.0]
    if direction == "forward":
        array[0] = GAIT_RANGE
    elif direction == "backward":
        array[0] = -1 * GAIT_RANGE
    elif direction == "rotation":
        array[2] = ROTATION_RANGE if angle > 0 else -1 * ROTATION_RANGE
        stepnum = int(abs(angle) / ROTATION_RANGE)
    else:
        rospy.logerr("error walk direction")
    for _ in range(stepnum):
        SendGaitCommand(array[0], array[1], array[2])


def move(mean_distance):
    print(mean_distance)
    if mean_distance < 150:
        print("离得太近了，我识别不到了。")
        return
    elif mean_distance > 1000:
        print("前方障碍物比较远，我可以走的快一些")
        slow_walk("forward", 4)
    elif mean_distance > 500:
        print("我准备往前走了。")
        slow_walk("forward", 1)
    elif mean_distance < 250:
        print("有点近，我需要后退一下")
        slow_walk("backward", 1)
    else:
        print("前方有障碍物，我准备右转30度")
        slow_walk("rotation", angle=-30)
    WaitForWalkingDone()


def callback(image):
    # 将ros中的图像信息转换成opencv的图像格式
    cv_image = bridge.imgmsg_to_cv2(image, "16UC1") # 16UC1为16位无符号整数

    cv_image = np.array(cv_image) #np.array()函数被用于将从ROS消息转换而来的OpenCV格式图像cv_image转换为NumPy数组进行进一步处理。
    height, width = cv_image.shape #依次返回图像的高和宽
    roi_image = cv_image[height / 2 - ROI[1] / 2: height / 2 + ROI[1] / 2,
                         width / 2 - ROI[0] / 2: width / 2 + ROI[0] / 2] # 图像中间框出一个长100宽100的图像
    mean_distance = roi_image.mean() # roi_image为一个np对象 mean为计算平均值
    return mean_distance #也为np对象


if __name__ == "__main__":
    rospy.init_node('roban_avoidance')# 发布节点名称
    rospy.Subscriber('terminate_current_process', String, terminate) #订阅节点
    bridge = CvBridge() #cv_bridge是在ROS图像消息和OpenCV图像之间进行转换的一个功能包。
    topic = "/camera/depth/image_rect_raw" # 订阅深度摄像头话题
    print SetBodyhubTo_walking(2)

    while not rospy.is_shutdown():#不发布停止指令就一直运行
        means_distance = []#list
        for _ in range(5):
            means_distance.append(callback(rospy.wait_for_message(topic, Image)))#返回框出图的平均值
        move(sum(means_distance) / len(means_distance))#传入mean_distance list的平均值

